/**
 ****************************(C) COPYRIGHT 2019 DJI****************************
* @file       shoot.c/h
* @brief      射击功能.
* @note
* @history
*  Version    Date            Author          Modification
*  V1.0.0     Dec-26-2018     RM              1. 完成
*
@verbatim
==============================================================================

==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2019 DJI****************************
*/

#include "shoot.h"
#include "main.h"

#include "cmsis_os.h"

#include "bsp_laser.h"
#include "bsp_fric.h"
#include "arm_math.h"
#include "user_lib.h"
#include "referee.h"

#include "CAN_receive.h"
#include "gimbal_behaviour.h"
#include "detect_task.h"
#include "pid.h"

#include "referee.h"
#include "vision_task.h"

#define shoot_fric1_on(pwm) fric1_on((pwm)) // 摩擦轮1pwm宏定义
#define shoot_fric2_on(pwm) fric2_on((pwm)) // 摩擦轮2pwm宏定义
#define shoot_fric_off() fric_off()         // 关闭两个摩擦轮

/*射击状态机设置，遥控器上拨一次开启，再上拨关闭，下拨1次发射1颗，一直处在下，则持续发射，用于3min准备时间清理子弹*/
static void shoot_set_mode(void);
/*射击数据更新*/
static void shoot_feedback_update(void);
/*堵转倒转处理*/
static void trigger_motor_turn_back(void);

shoot_control_t shoot_control; // 射击数据

int shoot_time = 0;
int ready_time = 0;

/*发射机构任务*/
void shoot_task(void const *pvParameters)
{
    // 射击初始化
    shoot_init();
    while (1)
    {
        vTaskDelay(2);
        shoot_set_mode();        // 模式设置
        shoot_feedback_update(); // 更新
        shoot_control_loop();    // 射击循环
    }
}

/*@brief          射击初始化，初始化PID，遥控器指针，电机指针*/
void shoot_init(void)
{
    static const fp32 Trigger_speed_pid[3] = {TRIGGER_ANGLE_PID_KP, TRIGGER_ANGLE_PID_KI, TRIGGER_ANGLE_PID_KD};
    shoot_control.shoot_mode = SHOOT_STOP;
    // 遥控器指针
    shoot_control.shoot_rc = get_remote_control_point();
    // 电机指针
    shoot_control.shoot_motor_measure = GetDjiMotorMeasurePoint(2, 6);
    // 初始化PID
    PID_init(&shoot_control.trigger_motor_pid, PID_POSITION, Trigger_speed_pid, TRIGGER_READY_PID_MAX_OUT, TRIGGER_READY_PID_MAX_IOUT);
    // 更新数据
    shoot_feedback_update();
    // 斜坡
    ramp_init(&shoot_control.fric1_ramp, SHOOT_CONTROL_TIME * 0.001f, FRIC_UP, FRIC_OFF);
    ramp_init(&shoot_control.fric2_ramp, SHOOT_CONTROL_TIME * 0.001f, FRIC_UP, FRIC_OFF);
    // 初始化相关变量
    shoot_control.fric_pwm1 = FRIC_OFF;
    shoot_control.fric_pwm2 = FRIC_OFF;
    shoot_control.ecd_count = 0;
    shoot_control.angle = shoot_control.shoot_motor_measure->ecd * MOTOR_ECD_TO_ANGLE;
    shoot_control.given_current = 0;
    shoot_control.move_flag = 0;
    shoot_control.set_angle = shoot_control.angle;
    shoot_control.speed = 0.0f;
    shoot_control.speed_set = 0.0f;
}

static void shoot_set_mode(void)
{


    // 上，拨弹，用于调试
    if (switch_is_up(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
    {
        // 设置让摩擦轮速度达到一定值之后再进行发弹
        if (shoot_control.fric_pwm1 >= SHOOT_CRITICAL_SPEED &&
            shoot_control.fric_pwm2 >= SHOOT_CRITICAL_SPEED)
            shoot_control.shoot_mode = SHOOT_FIRE;
    }
    // 中，开启摩擦轮---上场直接开启摩擦轮，就不关了
    else if (switch_is_mid(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) && get_robot_shoot_power())
    {
        // 等待620上电
        if (shoot_time < 20)
        {
            vTaskDelay(2);
            shoot_time++;
        }
        else
        {
            shoot_control.shoot_mode = SHOOT_READY_FRIC;
            shoot_time = 0;
        }
    }
    // 下，停止
    else if (switch_is_down(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) || !get_robot_shoot_power())
    {
        shoot_control.shoot_mode = SHOOT_STOP;
    }

    // 当前状态准备好射击
    if (shoot_control.shoot_mode == SHOOT_READY_FRIC &&
        (shoot_control.fric_pwm1 >= SHOOT_CRITICAL_SPEED &&
         shoot_control.fric_pwm2 >= SHOOT_CRITICAL_SPEED))
    {
        // 添加游戏状态等判断
        if (shoot_control.game_process_now == 4) // 设置游戏进行中再进行进一步操作
        {
            // 根据是否瞄准目标进行判断，并添加刚瞄准后枪口稳定的等待时间
            if (get_tracking_status() == 0)
            {
                ready_time = 0;
            }
            else
            {
                if (ready_time < 20)
                {
                    vTaskDelay(1);
                    ready_time++;
                }
                else
                {
                    shoot_control.shoot_mode = SHOOT_FIRE;
                }
            }
        }
    }

    // 发弹超热量
    if (!toe_is_error(REFEREE_TOE) && (shoot_control.heat + SHOOT_HEAT_REMAIN_VALUE > shoot_control.heat_limit))
    {
        if (shoot_control.shoot_mode == SHOOT_FIRE)
        {
            shoot_control.shoot_mode = SHOOT_READY_FRIC;
        }
    }

    // 如果没有发弹量直接不发弹
    if (get_allowance() <= 2 && shoot_control.shoot_mode == SHOOT_FIRE &&
        switch_is_mid(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
    {
        shoot_control.shoot_mode = SHOOT_READY_FRIC;
    }
}

int16_t shoot_control_loop(void)
{
    // -----------根据模式设置2006拨弹电机的速度--------------
    if (shoot_control.shoot_mode == SHOOT_STOP)
    {
        // 设置拨弹轮的速度
        shoot_control.speed_set = 0.0f;
    }
    else if (shoot_control.shoot_mode == SHOOT_READY_FRIC)
    {
        // 设置拨弹轮的速度
        shoot_control.speed_set = 0.0f;
    }
    // 射击
    else if (shoot_control.shoot_mode == SHOOT_FIRE)
    {
        // 设置拨弹轮的拨动速度,并开启堵转反转处理
        shoot_control.trigger_speed_set = CONTINUE_TRIGGER_SPEED;
        trigger_motor_turn_back();
    }

    // ---------------跟据模式设置摩擦轮速度----------
    if (shoot_control.shoot_mode == SHOOT_STOP) // 关闭摩擦轮
    {
        shoot_control.given_current = 0;
        // 关闭摩擦轮
        shoot_control.fric_pwm1 = 0;
        shoot_control.fric_pwm2 = 0;
        shoot_control.fric1_ramp.out = 0;
        shoot_control.fric2_ramp.out = 0;
    }
    else
    {

        // vTaskDelay(100);

        // 计算拨弹轮电机PID
        PID_calc(&shoot_control.trigger_motor_pid, shoot_control.speed, shoot_control.speed_set);
        shoot_control.given_current = (int16_t)(shoot_control.trigger_motor_pid.out);
        if (shoot_control.shoot_mode == SHOOT_READY_FRIC)
        {
            shoot_control.given_current = 0;
        }

        ramp_calc(&shoot_control.fric1_ramp, SHOOT_FRIC_PWM_ADD_VALUE);
        ramp_calc(&shoot_control.fric2_ramp, SHOOT_FRIC_PWM_ADD_VALUE);

        shoot_control.fric_pwm1 = (uint16_t)(shoot_control.fric1_ramp.out);
    shoot_control.fric_pwm2 = (uint16_t)(shoot_control.fric2_ramp.out);
    
    }

    shoot_fric1_on(shoot_control.fric_pwm1);
    shoot_fric2_on(shoot_control.fric_pwm2);

    return shoot_control.given_current;
}

/*射击数据更新*/
static void shoot_feedback_update(void)
{

    shoot_control.heat = get_heat_auto();
    shoot_control.heat_limit = get_heat_limit();
    shoot_control.game_process_now = get_game_status_progress();

    static fp32 speed_fliter_1 = 0.0f;
    static fp32 speed_fliter_2 = 0.0f;
    static fp32 speed_fliter_3 = 0.0f;

    // 拨弹轮电机速度滤波一下
    static const fp32 fliter_num[3] = {1.725709860247969f, -0.75594777109163436f, 0.030237910843665373f};

    // 二阶低通滤波
    speed_fliter_1 = speed_fliter_2;
    speed_fliter_2 = speed_fliter_3;
    speed_fliter_3 = speed_fliter_2 * fliter_num[0] + speed_fliter_1 * fliter_num[1] + (shoot_control.shoot_motor_measure->speed_rpm * MOTOR_RPM_TO_SPEED) * fliter_num[2];
    shoot_control.speed = speed_fliter_3;

    // 电机圈数重置， 因为输出轴旋转一圈， 电机轴旋转 36圈，将电机轴数据处理成输出轴数据，用于控制输出轴角度
    if (shoot_control.shoot_motor_measure->ecd - shoot_control.shoot_motor_measure->last_ecd > HALF_ECD_RANGE)
    {
        shoot_control.ecd_count--;
    }
    else if (shoot_control.shoot_motor_measure->ecd - shoot_control.shoot_motor_measure->last_ecd < -HALF_ECD_RANGE)
    {
        shoot_control.ecd_count++;
    }

    if (shoot_control.ecd_count == FULL_COUNT)
    {
        shoot_control.ecd_count = -(FULL_COUNT - 1);
    }
    else if (shoot_control.ecd_count == -FULL_COUNT)
    {
        shoot_control.ecd_count = FULL_COUNT - 1;
    }

    // 计算输出轴角度
    shoot_control.angle = (shoot_control.ecd_count * ECD_RANGE + shoot_control.shoot_motor_measure->ecd) * MOTOR_ECD_TO_ANGLE;

    // 射击开关下档时间计时
    if (shoot_control.shoot_mode != SHOOT_STOP && switch_is_down(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
    {

        if (shoot_control.rc_s_time < RC_S_LONG_TIME)
        {
            shoot_control.rc_s_time++;
        }
    }
    else
    {
        shoot_control.rc_s_time = 0;
    }
}

// 卡弹，倒退
static void trigger_motor_turn_back(void)
{
    if (shoot_control.block_time < BLOCK_TIME)
    {
        shoot_control.speed_set = shoot_control.trigger_speed_set;
    }
    else
    {
        shoot_control.speed_set = -shoot_control.trigger_speed_set;
    }

    if (fabs(shoot_control.speed) < BLOCK_TRIGGER_SPEED && shoot_control.block_time < BLOCK_TIME)
    {
        shoot_control.block_time++;
        shoot_control.reverse_time = 0;
    }
    else if (shoot_control.block_time == BLOCK_TIME && shoot_control.reverse_time < REVERSE_TIME)
    {
        shoot_control.reverse_time++;
    }
    else
    {
        shoot_control.block_time = 0;
    }
}
